package gui.views.painter;


import gui.views.OccupancyMapView;

import java.awt.*;

/**
 * Project : Leertaak 4 Mobile Robot Explorer
 * Package : gui.views
 * <p/>
 * User: Dustin Meijer
 * Date: 13-12-12
 * Time: 19:35
 */
public class LaserPainter extends ControlWindowPainter {

	private int[][] grid;

	public void paint(Graphics g, OccupancyMapView mapView) {

	}

	//orgineel in occupancy map
	public void drawLaserScan(double position[], double measures[]) {
		double rx = Math.round(position[0] + 20.0 * Math.cos(Math.toRadians(position[2])));
		double ry = Math.round(position[1] + 20.0 * Math.sin(Math.toRadians(position[2])));
		int dir = (int) Math.round(position[2]);

		for (int i = 0; i < 360; i++) {
			int d = i - dir;
			while (d < 0) {
				d += 360;
			}
			while (d >= 360) {
				d -= 360;
			}
			double fx = Math.round(rx + measures[d] * Math.cos(Math.toRadians(i)));
			double fy = Math.round(ry + measures[d] * Math.sin(Math.toRadians(i)));

			if (measures[d] < 100) {
				drawLaserBeam(rx, ry, fx, fy, true);
			} else {
				drawLaserBeam(rx, ry, fx, fy, false);
			}
		}
		// TODO : Allow the view to repaint itself
		//this.repaint();
	}

	//orgineel in occupancy map
	private void drawLaserBeam(double rx, double ry, double x, double y, boolean obstacle) {
		int rxi = (int) Math.ceil(rx / CELL_DIMENSION);
		int ryj = (int) Math.ceil(ry / CELL_DIMENSION);
		int xi = (int) Math.ceil(x / CELL_DIMENSION);
		int yj = (int) Math.ceil(y / CELL_DIMENSION);

		if (xi < 0 || yj < 0 || xi >= MAP_WIDTH / CELL_DIMENSION || yj >= MAP_HEIGHT / CELL_DIMENSION)
			return;

		if (obstacle) {
			grid[xi][yj] = OBSTACLE;
		} else if (grid[xi][yj] != OBSTACLE) {
			grid[xi][yj] = EMPTY;
		}

		int xmin = Math.min(rxi, xi);
		int xmax = Math.max(rxi, xi);
		int ymin = Math.min(ryj, yj);
		int ymax = Math.max(ryj, yj);

		if (rx == x) {
			for (int j = ymin; j <= ymax; j++) {
				if (grid[xmin][j] != OBSTACLE)
					grid[xmin][j] = EMPTY;
			}
		} else {
			double m = (y - ry) / (x - rx);
			double q = y - m * x;
			for (int i = xmin; i <= xmax; i++) {
				int h = (int) Math.ceil((m * (i * CELL_DIMENSION) + q) / CELL_DIMENSION);
				if (h >= ymin && h <= ymax) {
					if (grid[i][h] != OBSTACLE) {
						grid[i][h] = EMPTY;
					}
//					if(grid[i+1][h] != OBSTACLE)
//					grid[i+1][h] = EMPTY;
				}
			}
		}
	}

}
